#include <yarp/os/Network.h>
#include <yarp/os/RFModule.h>
#include <yarp/os/Bottle.h>
#include <yarp/os/BufferedPort.h>
#include <yarp/os/RpcClient.h>
#include <yarp/os/Time.h>

#include <yarp/sig/Matrix.h>
#include <yarp/math/Math.h>

#include <gsl/gsl_math.h>

#include <iostream>
#include <iomanip>
#include <string>

using namespace std;
using namespace yarp::os;
using namespace yarp::sig;
using namespace yarp::math;

class Launcher: public RFModule
{
private:
    RpcClient port0, port1,port2,port3;
	
    bool init;    
    double x0;
    double y0;
    double z0;
    int cnt;

	BufferedPort<Bottle> inPort;
	bool simulator;

public:

    bool configure(ResourceFinder &rf)
    {
        Time::turboBoost();

        string name=rf.find("name").asString().c_str();
		port0.open(("/"+name+"0:o").c_str());
        port1.open(("/"+name+"1:o").c_str());
		port2.open(("/"+name+"2:o").c_str());
		port3.open(("/"+name+"3:o").c_str());

        if (!Network::connect(port0.getName().c_str(),"/icubSim/world") 
			|| !Network::connect(port1.getName().c_str(),"/icubSim/world")
			|| !Network::connect(port2.getName().c_str(),"/icubSim/world") 
			|| !Network::connect(port3.getName().c_str(),"/icubSim/world"))
        {
            cout<<"unable to connect to the world!"<<endl;
			port0.close();
            port1.close();
			port2.close();
			port3.close();

            return false;
        }

		inPort.open(("/"+name+":i").c_str());  // need for someone writing on that port
		
        x0=0.0;
        y0=1.0;
        z0=0.75;

        init=true;        
        return true;
    }

    bool close()
    {
        port0.interrupt();
		port1.interrupt();
		port2.interrupt();
		port3.interrupt();

		port0.close();
        port1.close();
		port2.close();
		port3.close();
		inPort.close();
        return true;
    }

    bool updateModule()
    {
        Bottle cmd,reply;
		Bottle cmd0,reply0;
		Bottle cmd1,reply1;
		Bottle cmd2,reply2;
		Bottle cmd3,reply3;

        if (init)
        {         	
			cmd.addString("world");
			cmd.addString("del");
			cmd.addString("all");
			port1.write(cmd,reply);
				
            cmd0.addString("world");
            cmd0.addString("mk");
            cmd0.addString("ssph");
            cmd0.addDouble(0.01);
            cmd0.addDouble(x0);
            cmd0.addDouble(y0);
            cmd0.addDouble(z0);
            cmd0.addDouble(1.0);
            cmd0.addDouble(0.0);
            cmd0.addDouble(0.0);
            cnt=0;

			cmd1.addString("world");
			cmd1.addString("mk");
			cmd1.addString("scyl");
			cmd1.addDouble(0.005);   // radius
			cmd1.addDouble(0.1);    // height
			cmd1.addDouble(x0);	  
			cmd1.addDouble(y0);     
			cmd1.addDouble(z0);
			cmd1.addDouble(0.0);
			cmd1.addDouble(1.0);
			cmd1.addDouble(0.0);

			cmd2.addString("world");
			cmd2.addString("mk");
			cmd2.addString("scyl");
			cmd2.addDouble(0.005);   // radius
			cmd2.addDouble(0.15);    // height
			cmd2.addDouble(x0);	  
			cmd2.addDouble(y0);     
			cmd2.addDouble(z0);
			cmd2.addDouble(0.0);
			cmd2.addDouble(0.0);
			cmd2.addDouble(1.0);
			
			cmd3.clear();
			cmd3.addString("world");
			cmd3.addString("rot");
			cmd3.addString("scyl");
			cmd3.addInt(1);
			cmd3.addDouble(0.0);   
			cmd3.addDouble(90);	  
			cmd3.addDouble(0.0); 

            init=false;
        }
        else
        {	
			
			Bottle *pos = inPort.read();
			Vector point(4);
			Vector rot(3);

			point(0) = pos->get(0).asDouble()*0.01;
			point(1) = pos->get(1).asDouble()*0.01;
			point(2) = pos->get(2).asDouble()*0.01;
			point(3) = 1;

			// Rotation of the object respect to reference frame axis
			rot(2) = pos->get(3).asDouble();   // around x
			rot(1) = pos->get(4).asDouble();   // around y
			rot(0) = pos->get(5).asDouble();   // around z
			//rot(0) = rot(0)+45.0;
			//if(rot(0) > 360.0) rot(0) = 0.0;

			Matrix Robot2Sim(4,4);
			Robot2Sim.zero();
			Robot2Sim(0,1) = -1;
			Robot2Sim(1,2) = 1;
			Robot2Sim(1,3) = 0.5976;
			Robot2Sim(2,0) = -1;
			Robot2Sim(2,3) = -0.026;
			Robot2Sim(3,3) = 1;

			//point = Robot2Sim*point;
			x0 = point(0);
			y0 = point(1);
			z0 = point(2);

            cmd.addString("world");
            cmd.addString("set");
            cmd.addString("ssph");
            cmd.addInt(1);
            cmd.addDouble(x0);
            cmd.addDouble(y0);
            cmd.addDouble(z0);

			port1.write(cmd,reply);

			cmd0.addString("world");
            cmd0.addString("set");
            cmd0.addString("scyl");
            cmd0.addInt(1);
            cmd0.addDouble(x0);
            cmd0.addDouble(y0);
            cmd0.addDouble(z0);

			cmd1.addString("world");
            cmd1.addString("set");
            cmd1.addString("scyl");
            cmd1.addInt(2);
            cmd1.addDouble(x0);
            cmd1.addDouble(y0);
            cmd1.addDouble(z0);

			cmd2.addString("world");
			cmd2.addString("rot");
			cmd2.addString("scyl");
			cmd2.addInt(2);
			cmd2.addDouble(rot(0));   
			cmd2.addDouble(rot(1));	  
			cmd2.addDouble(rot(2)); 
			
			cmd3.addString("world");
			cmd3.addString("rot");
			cmd3.addString("scyl");
			cmd3.addInt(1);
			cmd3.addDouble(rot(0));   
			cmd3.addDouble(90+rot(1));	  
			cmd3.addDouble(rot(2));  
        }

		port0.write(cmd0,reply0);
		port1.write(cmd1,reply1);
        port2.write(cmd2,reply2);
		port3.write(cmd3,reply3);
        return true;
    }

    double getPeriod()
    {
        return 0.1;
    }
};



int main(int argc, char *argv[])
{
    Network yarp;
    if (!yarp.checkNetwork())
        return -1;

    ResourceFinder rf;
    rf.setVerbose(true);
    rf.setDefault("name","mover");
    rf.configure("ICUB_ROOT",argc,argv);

    Launcher launcher;
    return launcher.runModule(rf);
}


